#ifndef MAVPROXY_FSM
#define MAVPROXY_FSM

#include "drone_defs.h"
#include <ros/ros.h>
#include <ros/assert.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/CommandBool.h>




// 状态机类
class StateMachine {
private:
    State* current_state;

public:
    StateMachine(State* state) : current_state(state) {}

    ~StateMachine() {
        delete current_state;
    }

    void Run() {
        while (true) {
            current_state->Handle();
            State* next_state = current_state->Next();
            delete current_state; // 删除当前状态
            current_state = next_state; // 切换到下一个状态
        }
    }
};

int main() {
    StateMachine machine(new State1()); // 创建状态机，初始状态为State1
    machine.Run(); // 运行状态机

    return 0;
}

#endif